/*
 * TestWander.java
 *
 * Created on 13 de febrero de 2008, 11:33 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.VectorAdditionArbitrator;
import Behaviors.Schemas.Motor.MSWander;
import Controllers.BehaviorBasedController;
import Robot.*;
import Util.*;
/**
 *
 * @author alejo
 */
public class TestWander {
    
    /** Creates a new instance of TestWander */
    
    public static void process() {
        Robot NxMarlin=new Robot("192.168.1.51",6665,"NxMarlin",0);
        
        RealPosition2d positionMarlin;
        try{
            positionMarlin=new RealPosition2d(NxMarlin,"positionMarlin",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        //*********************************************************

        NxMarlin.runThreaded(10,0);
            //Formacion formacion=new Formacion();


        positionMarlin.setInitialPosition(new Pose2d(0,0,(float)Math.toRadians(90.0f)));
            //positionCenter.setInitialPosition(new Pose2d(-6.5f,-6.5f,0.79f));
        ///Vector2d point=new Vector2d(1,1,Vector2d.MODE_XY);

        //System.out.println("PointAngle="+Float.toString(point.getTheta()));

        MSWander wander=new MSWander("goto2");
        wander.setGain(1.0f);
            //DetectFormation detectFormation=new DetectFormation(position1, positionCenter);
            //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(wander,positionMarlin, false);

        BehaviorBasedController controller=new BehaviorBasedController("prueba",NxMarlin,arbitrator1,10);


        Thread hilo=new Thread(controller);
        hilo.start();
    
    
    
    }
}
